/*
 * ahrsfilter.h
 *
 *  Created on: Dec 11, 2012
 *      Author: mark
 */

#ifndef AHRSFILTER_H_
#define AHRSFILTER_H_

#include <math.h>

#include "../iir/iirfilter.h"

/**
 * Definition of quaternions.
 */
struct Quaternions {
	double w, x, y, z;
};

/**
 * AHRS filter class definition.
 */
class AHRSFilter {
private:
	static	const	double		deltat			=	0.04;
	static	const	double		gyroMeasError	=	30;

	Quaternions quaternions;
	double 		SEq_1;
	double		SEq_2;
	double		SEq_3;
	double		SEq_4;
	double		beta;
	IIRFilter	*HPfilterp;
	IIRFilter	*HPfilterq;
	IIRFilter	*HPfilterr;

public:
	AHRSFilter();
	virtual ~AHRSFilter();

	Quaternions filterupdate(
			// Angular rate
			double w_x,
			double w_y,
			double w_z,

			// Acceleration
			double a_x,
			double a_y,
			double a_z
			);
};

#endif /* AHRSFILTER_H_ */
